#ifndef HEXAPOD_SERVO_H
#define HEXAPOD_SERVO_H

#include <stdbool.h>
#include <stdint.h>

//                   \tail/
//  id14, id16, id18 |body| id17, id15, id13
//  id8, id10, id12  |body| id11,  id9, id7
//  id2,  id4,  id6  |body| id5,  id3,  id1 
//                   \head/
//  

#define ServoType_BUS 1u
#define ServoType_PWM 2u

typedef struct Servo {
	uint8_t id;
	uint8_t type; 
	float angle;
	float angleOffset;
	float second;
	float maxAngle;
	uint16_t maxTicks;
	int8_t direction;
} SERVO;
extern const SERVO SERVOS[20];

void setServoPosition(uint8_t servo_id, uint16_t pos, uint16_t duration);
void setServoDeviation(uint8_t servo_id, int16_t value);
void stopServo(uint8_t servo_id);
void setServoID(uint8_t old_id, uint8_t new_id);
bool readServoID(uint8_t servo_id, int16_t* value, int count);
void saveServoDeviation(uint8_t servo_id);
bool readServoDeviation(uint8_t servo_id, int16_t* value, int count);
void setServoAngleLimit(uint8_t servo_id, uint8_t low, uint8_t high);
bool readServoAngleLimit(uint8_t servo_id, int16_t* value, int count);
void setServoVinLimit(uint8_t servo_id, int16_t low, int16_t high);
bool readServoVinLimit(uint8_t servo_id, int16_t* value, int count);
void setServoMaxTemp(uint8_t servo_id, int16_t m_temp);
bool readServoTempLimit(uint8_t servo_id, int16_t* value, int count);
bool readServoPos(uint8_t servo_id, int16_t* value, int count);
bool readServoPosition(uint8_t servo_id, int16_t* value, int count);
bool readServoTemp(uint8_t servo_id, int16_t* value, int count);
bool readServoVin(uint8_t servo_id, int16_t* value, int count);
void resetServoPos(uint8_t old_id);
void writeServoTau(uint8_t servo_id, int16_t new_state);
bool readServoTau(uint8_t servo_id, int16_t* value, int count);
void setServoAngle(SERVO* servo);

void test_servo_state();


#endif

